#include "publish.h"
#include "livox_gflags.h"

#include <cmath>

namespace apollo {
namespace drivers {
namespace livox {
bool WriterWrapper::Init(const WriterConfig config,
                         std::shared_ptr<Node> node) {
  config_ = config;
  node_ = node;
  if (node_ == nullptr) {
    AERROR << "Empty node!";
    return false;
  }

  writer_ = node_->CreateWriter<Cloud>(config_.channel_name());
  if (writer_ == nullptr) {
    AERROR << "Writer init failed!";
    return false;
  }
  return true;
}

void processCloud(std::shared_ptr<Cloud>& apollo_pc, const std::shared_ptr<Cloud>& cloud) {
  for (auto p : cloud->point()) {
    PointXYZIT* point = apollo_pc->add_point();
    point->set_x(p.x());
    point->set_y(p.y());
    point->set_z(p.z());
    point->set_intensity(uint32_t(p.intensity()));
    point->set_timestamp(uint64_t(p.timestamp()));
  }
}

void WriterWrapper::Write(const std::shared_ptr<MixLidarFrame> frame) {
  auto cloud_msg = std::make_shared<Cloud>();
  
  switch (config_.surround_mode()) {
    case FrontOnly:{
    processCloud(cloud_msg, frame->frontCloud);
    auto header = cloud_msg->mutable_header();
    header->set_frame_id(frame->frontCloud->header().frame_id());
    break;}
    case LeftOnly:{
    processCloud(cloud_msg, frame->leftCloud);
    auto header = cloud_msg->mutable_header();
    header->set_frame_id(frame->leftCloud->header().frame_id());
    break;}
    case RightOnly:{
    processCloud(cloud_msg, frame->rightCloud);
    auto header = cloud_msg->mutable_header();
    header->set_frame_id(frame->rightCloud->header().frame_id());
    break;}
    case RearOnly:{
    processCloud(cloud_msg, frame->rearCloud);
    auto header = cloud_msg->mutable_header();
    header->set_frame_id(frame->rearCloud->header().frame_id());
    break;}
  }

  if (cloud_msg->point_size() > 0) {
    auto header = cloud_msg->mutable_header();
    header->set_timestamp_sec(cyber::Time::Now().ToSecond());
    header->set_lidar_timestamp(frame->endStamp);
    header->set_sequence_num(frame->seq);
    cloud_msg->set_measurement_time(static_cast<double>(frame->endStamp) / 1e9);
    cloud_msg->set_width(cloud_msg->point_size());
    cloud_msg->set_height(1);
    cloud_msg->set_is_dense(true);
    writer_->Write(cloud_msg);
    // double time_now = (double)apollo::cyber::Time::Now().ToNanosecond() / 1e9;
    // double pointcloud_time = static_cast<double>(frame->endStamp) / 1e9;
    // double diff = std::fabs(time_now - pointcloud_time);
    // static double last_pointcloud_time = 0;
    // AINFO(
    //     "publish {} pointcloud, system time is {}, timestamp is {}, pointsize "
    //     "is {}, channel is {}, nan ratio is {}",
    //     config_.fusion_mode(), time_now, pointcloud_time,
    //     cloud_msg->point_size(), config_.channel_name(),
    //     (double)cloud_msg->nan_point_size() / cloud_msg->point_size());
    // if (diff > 0.3) {
    //     AINFO(
    //         "{} pointcloud has latency for {}, system time is {}, pointcloud "
    //         "time is {}, channel is {}.",
    //         config_.fusion_mode(), diff, time_now, pointcloud_time,
    //         config_.channel_name());
    // }
    // if (last_pointcloud_time != 0 &&
    //     pointcloud_time - last_pointcloud_time > 0.2) {
    //     AWARN(
    //         "{} pointcloud lost frame, last timestamp is {}, current timestamp "
    //         "is {}, channel is {}.",
    //         config_.fusion_mode(), last_pointcloud_time, pointcloud_time,
    //         config_.channel_name());
    // }
    // last_pointcloud_time = pointcloud_time;
  } 
}

bool Publisher::Init(std::shared_ptr<Node> node) {
  if (!apollo::cyber::common::GetProtoFromFile(FLAGS_publish_config_file,
                                                  &config_)) {
    AERROR << "Failed to load config file: " << FLAGS_publish_config_file;
    return false;
  }

  int writerNum = config_.writer_size();
  for (int i = 0; i < writerNum; i++) {
    auto writer = std::make_shared<WriterWrapper>();
    auto &config = config_.writer(i);
    if (!writer->Init(config, node)) {
      AERROR << "Writer init failed!";
      return false;
    }
    writer_list_.emplace_back(writer);
  }
  return true;
}

void Publisher::Publish(const std::shared_ptr<MixLidarFrame> frame) {
  for (int i = 0; i < writer_list_.size(); i++) {
    writer_list_[i]->Write(frame);
  }
}

}  // namespace livox
}  // namespace drivers
}  // namespace apollo

